#include "robotjointmotiontrack.h"

RobotJointMotionTrack::RobotJointMotionTrack(Robot* robot, int jointID, QObject *parent) :
    QVariantAnimation(parent),
    _robot(robot),
    _jointID(jointID),
    _prevTime(0)
{
    setKeyFrameAt(0,0);
    setKeyFrameAt(duration(), 0);
}

RobotJointMotionTrack::~RobotJointMotionTrack(){
}

void RobotJointMotionTrack::setRobot(Robot *robot) {
    _robot = robot;
}

int RobotJointMotionTrack::jointID() const{
    return _jointID;
}

void RobotJointMotionTrack::updateCurrentValue(const QVariant &value){
    //We don't need to use the command pattern here
    _robot->setJointAngle(_jointID, value.toFloat());
}

void RobotJointMotionTrack::updateCurrentTime(int msec){
    //Call the parent function
    QVariantAnimation::updateCurrentTime(msec);

    int i, keyFrameTimestamp;
    KeyValues keyFrames = keyValues();
    KeyValue curKeyFrame;

    //Check to see if we hit a keyframe
    for(i = 0; i < keyFrames.size(); i++){
        curKeyFrame = keyFrames[i];
        keyFrameTimestamp = convertToMsec(curKeyFrame.first);

        //We are either on a key frame, or we overshot it
        if((keyFrameTimestamp == msec) ||
           ((msec > keyFrameTimestamp) && (_prevTime < keyFrameTimestamp)))
            emit keyFrame(keyFrameTimestamp);
    }

    _prevTime = msec;
}

void RobotJointMotionTrack::setDuration(int msecs){
    //Replace the key frames w/ corrected ones to increase the
    //length of the track or clip it, rather than scaling the animation
    //(which is the default behavior due to "steps")
    int i;
    KeyValues keyFrames = keyValues(), newKeyFrames;
    KeyValue curKeyFrame, newKeyFrame;
    qreal oldTimestamp, correctedTimestamp, oldDuration = (qreal)duration();

    for(i = 0; i < keyFrames.count(); i++){
        curKeyFrame = keyFrames[i];
        oldTimestamp = curKeyFrame.first;

        //Ignore correcting the end value. Keep it at 1.0
        if(oldTimestamp < 1.0){
            correctedTimestamp = (oldTimestamp * oldDuration) / ((qreal)msecs);
            if(correctedTimestamp >= 0.0 && correctedTimestamp <= 1.0){
                newKeyFrame.first = correctedTimestamp;
                newKeyFrame.second = curKeyFrame.second;
                newKeyFrames.append(newKeyFrame);
            }
        }
        else if(oldTimestamp == 1.0){
            newKeyFrame.first = 1.0;
            newKeyFrame.second = curKeyFrame.second;
            newKeyFrames.append(newKeyFrame);
        }
    }

    //Do the default stuff to update the track properties, etc.
    QVariantAnimation::setDuration(msecs);
    //Replace the key values w/ the corrected ones
    setKeyValues(newKeyFrames);

    emit durationChanged(msecs);
}

float RobotJointMotionTrack::insertKeyFrameAt(int msec){
    int i;
    qreal prevTimestamp, nextTimestamp, progress, convertedTimestamp;
    QVariant interpolatedValue, prevKeyValue, nextKeyValue;
    KeyValues keyFrames = keyValues();
    KeyValue curKeyFrame, nextKeyFrame;

    //Check to see if a keyframe already exists at the position
    if(this->keyValueAt(msec) != INVALID_KEYFRAME){
        qDebug() << "RobotJointMotionTrack::insertKeyFrameAt: Key frame already exists";
        return INVALID_KEYFRAME;
    }

    convertedTimestamp = convertFromMsec(msec);
    if(convertedTimestamp > 1){
        //The insert is happening after the end of the track, set
        //the value to 0
        return setKeyFrameAt(msec, 0);
    }
    else if(convertedTimestamp > 0 && convertedTimestamp < 1){

        for(i = 0; i < keyFrames.count() - 1; i++){
            //Find where the new keyframe belongs
            curKeyFrame = keyFrames[i];
            nextKeyFrame = keyFrames[i+1];
            if((convertedTimestamp > curKeyFrame.first) &&
               (convertedTimestamp < nextKeyFrame.first)){
                //Interpolate the value between the previous and next keyframe
                prevTimestamp = curKeyFrame.first;
                prevKeyValue = curKeyFrame.second.toFloat();
                nextTimestamp = nextKeyFrame.first;
                nextKeyValue = nextKeyFrame.second.toFloat();

                progress = (convertFromMsec(msec) - prevTimestamp) / (nextTimestamp - prevTimestamp);
                interpolatedValue = interpolated(prevKeyValue, nextKeyValue, progress);

                return setKeyFrameAt(msec, interpolatedValue.toFloat());
            }
        }
    }

    return INVALID_KEYFRAME;
}


float RobotJointMotionTrack::setKeyFrameAt(int msec, float theta){
    qreal convertedTimestamp = convertFromMsec(msec), oldEndStep;
    float min, max, correctedAngle;
    QVariant oldEndValue;

    if(convertedTimestamp >= 1){
        //This new key frame is going to be the end value
        oldEndStep = ((qreal)duration()) / ((qreal)msec);
        oldEndValue = endValue();

        //Increase the duration of the track
        setDuration(msec);

        //Override the default setDuration behavior and move the
        //old end value back to where it was rather than keeping
        //it as the end value
        setKeyValueAt(oldEndStep, oldEndValue);

        //Recalculate the "step" position for the new key frame
        convertedTimestamp = convertFromMsec(msec);
    }
    Q_ASSERT(convertedTimestamp <= 1);

    if(convertedTimestamp >= 0.0 && convertedTimestamp <= 1.0){
        //correct for minimum and maximum values
        min = _robot->minimum(_jointID);
        max = _robot->maximum(_jointID);

        // Hard coded error values for when the robot doesn't support these joints
        if (min == INVALID_JOINT) {
            min = -360;
        }
        if (max == INVALID_JOINT) {
            max = 360;
        }

        correctedAngle = qBound(min, theta, max);

        QVariantAnimation::setKeyValueAt(convertedTimestamp, correctedAngle);
        return correctedAngle;
    }

    return INVALID_KEYFRAME;
}

float RobotJointMotionTrack::deleteKeyFrameAt(int msec){
    int i, keyFrameTimestamp;
    KeyValue curKeyFrame;
    //Get the current list of key values
    KeyValues keyFrames = keyValues();

    if(msec == 0)
        //You can't delete the start key frame
        return INVALID_KEYFRAME;

    //Remove the value and replace the list
    for(i = 0; i < keyFrames.size(); i++){
        curKeyFrame = keyFrames[i];
        keyFrameTimestamp = convertToMsec(curKeyFrame.first);

        if(keyFrameTimestamp == msec){
            keyFrames.remove(i);
            setKeyValues(keyFrames);

            if(msec == duration())
                //This was the ending key frame, we need
                //to update the duration of the track
                refreshDuration();

            return curKeyFrame.second.toFloat();
        }
    }

    //Doesn't exist
    return INVALID_KEYFRAME;
}

void RobotJointMotionTrack::refreshDuration(){
    //Search the key frames for the last one
    //TODO: If the keyValues() are always returned in
    //order, then we don't need to search
    int i;
    qreal newDuration = -1;
    KeyValues keyFrames = keyValues();

    for(i = 0; i < keyFrames.count(); i++){
        if(keyFrames[i].first > newDuration){
            newDuration = keyFrames[i].first;
        }
    }

    if(duration() != newDuration){
        setDuration(convertToMsec(newDuration));
    }
}


bool RobotJointMotionTrack::moveKeyFrame(int msec, int newMsec){
    int i, keyFrameTimestamp;
    float setValue = INVALID_KEYFRAME;
    KeyValue curKeyFrame;
    //Get the current list of key frames
    KeyValues keyFrames = keyValues();    

    for(i = 0; i < keyFrames.size(); i++){
        curKeyFrame = keyFrames[i];
        keyFrameTimestamp = convertToMsec(curKeyFrame.first);

        if(keyFrameTimestamp == msec){
            //Remove the existing key frame
            keyFrames.remove(i);
            setKeyValues(keyFrames);
            setValue = setKeyFrameAt(newMsec, curKeyFrame.second.toFloat());

            if(msec == duration()){
                refreshDuration();
            }
            if(setValue != INVALID_KEYFRAME)
                return true;
            else
                return false;
        }
    }

    return false;
}

float RobotJointMotionTrack::keyValueAt(int msec) const{
    QVariant keyValue = QVariantAnimation::keyValueAt(convertFromMsec(msec));
    if(keyValue.isValid()){
        return keyValue.toFloat();
    }
    else{
        return INVALID_KEYFRAME;
    }
}

QList< QPair<int, float> > RobotJointMotionTrack::keyFrames() const{
    int i;
    QList< QPair<int, float> > retList;
    KeyValues keyFrames = keyValues();
    KeyValue curKeyFramePair;
    QPair<int, float> keyFramePair;

    //Essentially return the KeyValues vector, but
    //convert the "steps" to msec
    for(i = 1; i < keyFrames.count() - 1; i++){
        curKeyFramePair = keyFrames.at(i);

        keyFramePair.first = convertToMsec(curKeyFramePair.first);
        keyFramePair.second = curKeyFramePair.second.toFloat();
        retList.append(keyFramePair);
    }

    return retList;
}

int RobotJointMotionTrack::nextKeyFrame(int curTime) const{
    //Find the next key frame from the given time
    int i;
    qreal curValue, convertedTime = convertFromMsec(curTime), curMax = convertFromMsec(duration());
    KeyValues keyFrames = keyValues();

    for(i = 0; i < keyFrames.count(); i++){
        curValue = keyFrames[i].first;

        if(curValue > convertedTime && curValue <= curMax){
            curMax = curValue;
        }
    }

    return convertToMsec(curMax);
}

int RobotJointMotionTrack::prevKeyFrame(int curTime) const{
    //Find the previous key frame from the given time
    int i;
    qreal curValue, convertedTime = convertFromMsec(curTime), curMin = 0;
    KeyValues keyFrames = keyValues();

    for(i = 0; i < keyFrames.count(); i++){
        curValue = keyFrames[i].first;

        if(curValue < convertedTime && curValue >= curMin){
            curMin = curValue;
        }
    }

    return convertToMsec(curMin);
}



inline qreal RobotJointMotionTrack::convertFromMsec(int msec) const{
    return ((qreal)msec / (qreal)duration());
}

inline int RobotJointMotionTrack::convertToMsec(qreal step) const{
    return (int)(step * duration());
}
